//
// File: desired_vel_v2.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 13-Jan-2025 15:48:32
//

// Include Files
#include "desired_vel_v2.h"
#include <cmath>

// Function Definitions
//
// Arguments    : double dg
//                double dob
//                double sig
//                double sio
//                double beta
//                double aph
//                double b
//                double *vRd
//                double *vLd
// Return Type  : void
//
void desired_vel_v2(double dg, double dob, double sig, double sio, double beta,
                    double aph, double, double *vRd, double *vLd)
{
  static const double a[32]{0.95, 0.9,  0.28, 0.46, 0.24, 0.2,  0.19, 0.73,
                            0.92, 1.4,  0.14, 0.44, 1.09, 0.45, 0.89, 0.41,
                            0.97, 0.67, 0.1,  0.75, 0.87, 0.04, 1.02, 1.12,
                            0.92, 0.69, 0.7,  1.12, 0.84, 0.38, 0.63, 0.52};
  static const double c[8]{0.0,
                           10.0,
                           0.0,
                           3.0,
                           -1.5707963267948966,
                           1.5707963267948966,
                           -1.5707963267948966,
                           1.5707963267948966};
  static const double dv[4]{5.0, 0.75, 1.0, 1.0};
  double W[32];
  double firingStrengths[16];
  double MFs[8];
  double S[4];
  double Outputs[2];
  double d;
  double firing_total;
  int o;
  S[0] = dg;
  S[1] = dob;
  S[2] = sig;
  S[3] = sio;
  firing_total = ((dg + dob) + sig) + sio;
  for (int i{0}; i < 32; i++) {
    W[i] = a[i] * firing_total;
  }
  // coder.inline("never");
  //  参数初始化
  //  输入层节点数
  //  每个输入的隶属度函数数
  //  模糊规则数
  //  输出层节点数
  //  初始化静态变量
  //      persistent pre_firingStrengths;
  //      if isempty(pre_firingStrengths)
  //              pre_firingStrengths =zeros(1,numRules);
  //      end
  // 训练均值和标准差
  //      sigma=[4.3,1.6,1.3,1.3];%v,w
  // v,w
  //  隶属度层
  for (int i{0}; i < 4; i++) {
    double b_a;
    firing_total = dv[i];
    //  定义高斯隶属度函数的参数
    // 均值
    // v标准差
    o = i << 1;
    d = S[i];
    b_a = d - c[o];
    firing_total *= firing_total;
    MFs[o] = std::exp(-(b_a * b_a) / firing_total);
    //  定义高斯隶属度函数的参数
    // 均值
    // v标准差
    b_a = d - c[o + 1];
    MFs[o + 1] = std::exp(-(b_a * b_a) / firing_total);
  }
  //  模糊规则层
  for (int i{0}; i < 16; i++) {
    //  计算每条规则的激活程度（AND操作）
    switch (i + 1) {
    case 1:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(1,3),MFs(1,4)]);%near
      //                  near right right
      firingStrengths[i] = MFs[0] * MFs[2] * MFs[4] * MFs[6];
      break;
    case 2:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(1,3),MFs(2,4)]);%near
      //                  near right left
      firingStrengths[i] = MFs[0] * MFs[2] * MFs[4] * MFs[7];
      break;
    case 3:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(2,3),MFs(1,4)]);%near
      //                  near left right
      firingStrengths[i] = MFs[0] * MFs[2] * MFs[5] * MFs[6];
      break;
    case 4:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(1,2),MFs(2,3),MFs(2,4)]);%near
      //                  near left left
      firingStrengths[i] = MFs[0] * MFs[2] * MFs[5] * MFs[7];
      break;
    case 5:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(1,3),MFs(1,4)]);%near
      //                  far right right
      firingStrengths[i] = MFs[0] * MFs[3] * MFs[4] * MFs[6];
      break;
    case 6:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(1,3),MFs(2,4)]);%near
      //                  far right left
      firingStrengths[i] = MFs[0] * MFs[3] * MFs[4] * MFs[7];
      break;
    case 7:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(2,3),MFs(1,4)]);%near
      //                  far left right
      firingStrengths[i] = MFs[0] * MFs[3] * MFs[5] * MFs[6];
      break;
    case 8:
      //                  firingStrengths(r)=min([MFs(1,1),MFs(2,2),MFs(2,3),MFs(2,4)]);%near
      //                  far left left
      firingStrengths[i] = MFs[0] * MFs[3] * MFs[5] * MFs[7];
      break;
    case 9:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(1,3),MFs(1,4)]);%far
      //                  near right right
      firingStrengths[i] = MFs[1] * MFs[2] * MFs[4] * MFs[6];
      break;
    case 10:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(1,3),MFs(2,4)]);%far
      //                  near right left
      firingStrengths[i] = MFs[1] * MFs[2] * MFs[4] * MFs[7];
      break;
    case 11:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(2,3),MFs(1,4)]);%far
      //                  near left right
      firingStrengths[i] = MFs[1] * MFs[2] * MFs[5] * MFs[6];
      break;
    case 12:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(1,2),MFs(2,3),MFs(2,4)]);%far
      //                  near left left
      firingStrengths[i] = MFs[1] * MFs[2] * MFs[5] * MFs[7];
      break;
    case 13:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(1,3),MFs(1,4)]);%far
      //                  far right right
      firingStrengths[i] = MFs[1] * MFs[3] * MFs[4] * MFs[6];
      break;
    case 14:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(1,3),MFs(2,4)]);%far
      //                  far right left
      firingStrengths[i] = MFs[1] * MFs[3] * MFs[4] * MFs[7];
      break;
    case 15:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(2,3),MFs(1,4)]);%far
      //                  far left right
      firingStrengths[i] = MFs[1] * MFs[3] * MFs[5] * MFs[6];
      break;
    default:
      //                  firingStrengths(r)=min([MFs(2,1),MFs(2,2),MFs(2,3),MFs(2,4)]);%far
      //                  far left left
      firingStrengths[i] = MFs[1] * MFs[3] * MFs[5] * MFs[7];
      break;
    }
  }
  //      firingStrengths=round(firingStrengths,3);
  firing_total = firingStrengths[0];
  for (int i{0}; i < 15; i++) {
    firing_total += firingStrengths[i + 1];
  }
  for (int i{0}; i < 16; i++) {
    firingStrengths[i] /= firing_total;
  }
  //      if(pre_firingStrengths~=zeros(1,numRules))
  //          now_firingStrengths=0.8*now_firingStrengths+0.2*pre_firingStrengths;
  //      end
  //      now_firingStrengths=firingStrengths;
  //      time_delay=zeros(1,numRules);
  //      now_firingStrengths=(ones(1,numRules)-time_delay).*firingStrengths+time_delay.*pre_firingStrengths;
  // 后件层
  //      [rulesOn,i]=max(now_firingStrengths);
  //      Outputs=rulesOn*W(:,i);
  for (o = 0; o < 2; o++) {
    d = 0.0;
    for (int i{0}; i < 16; i++) {
      //  线性组合
      d += firingStrengths[i] * W[o + (i << 1)];
    }
    Outputs[o] = d;
  }
  // 速度输出
  // 最小转向半径
  *vRd = Outputs[0] / (beta + aph * std::abs(Outputs[0]));
  *vLd = Outputs[1] / (beta + aph * std::abs(Outputs[1]));
  //      v=(vR+vL)/2;
  //      w=(vR-vL)/(2*b);
  //      if(abs(v/w)<rmin)
  //          v=0.3;
  //          if(vR>vL)
  //              w=v/rmin;
  //              vR=v+b*w;
  //              vL=v-b*w;
  //          else
  //              w=-v/rmin;
  //              vR=v+b*w;
  //              vL=v-b*w;
  //          end
  //      end
}

//
// File trailer for desired_vel_v2.cpp
//
// [EOF]
//
